'''
1.0.0 版本 
# Date: 2024/12/28
# Author: ZHENG
# Description:
    # 此版本功能如下：
        1. 实现一个简单的二维栅格地图的可视化

'''

# -*- coding: utf-8 -*-
import sys
import numpy as np
from PyQt5.QtWidgets import QApplication, QMainWindow, QGraphicsView, QGraphicsScene
from PyQt5.QtGui import QImage, QColor, QPainter, QPen
from PyQt5.QtCore import Qt, QPointF, QRectF

# from PyQt5.QtConcurrent import run

class OccupancyMap:
    def __init__(self, data):
        self.data = data  # 这里假设data是一个二维numpy数组

    def Cols(self):
        return self.data.shape[1]

    def Rows(self):
        return self.data.shape[0]

    def __getitem__(self, key):
        return self.data[key]

    def xy2ScenePose(self, x, y):
        # 这里假设一个简单的转换，实际应用中可能需要更复杂的转换
        return x, y

class DisplayOccMap(QGraphicsView):
    def __init__(self, display_type, z_value, parent_name):
        super().__init__()
        self.display_type = display_type
        self.z_value = z_value
        self.parent_name = parent_name
        self.map_data_ = None
        self.map_image_ = QImage()
        self.line_start_pose_ = QPointF()
        self.line_tmp_image_ = QImage()
        self.is_draw_line_ = False
        self.setScene(QGraphicsScene(self))
        self.setRenderHint(QPainter.Antialiasing)

    def update_data(self, data):
        try:
            if isinstance(data, OccupancyMap):
                self.map_data_ = data
        except Exception as e:
            print(f"Error: {e}")
            return False

        self.parse_occupy_map()
        return True

    def set_display_config(self, config_name, config_data):
        if config_name == "SubMapValue":
            self.sub_map_value_ = config_data
        elif config_name == "RobotPose":
            self.sub_map_center_pose_ = config_data
        else:
            return False
        self.update()
        return True

    def paintEvent(self, event):
        super().paintEvent(event)
        if not self.map_image_.isNull():
            painter = QPainter(self)
            painter.drawImage(0, 0, self.map_image_)

    def parse_occupy_map(self):
        self._parse_occupy_map

    def _parse_occupy_map(self):
        map_data = self.map_data_
        self.map_image_ = QImage(map_data.Cols(), map_data.Rows(), QImage.Format_RGB32)
        for i in range(map_data.Cols()):
            for j in range(map_data.Rows()):
                map_value = map_data[j, i]
                if map_value == 100:
                    color = Qt.black
                elif map_value == 0:
                    color = Qt.white
                elif map_value == 255:
                    color = Qt.gray
                else:
                    color = Qt.white
                self.map_image_.setPixel(i, j, color.rgb())

        self.set_bounding_rect(QRectF(0, 0, self.map_image_.width(), self.map_image_.height()))
        self.update()

    def set_bounding_rect(self, rect):
        self.setSceneRect(rect)

    def erase_map_range(self, pose, range_):
        x, y = pose.x(), pose.y()
        left = max(0, int(x - range_))
        top = max(0, int(y - range_))
        right = min(self.map_image_.width() - 1, int(x + range_))
        bottom = min(self.map_image_.height() - 1, int(y + range_))

        for i in range(left, right + 1):
            for j in range(top, bottom + 1):
                self.map_image_.setPixelColor(i, j, Qt.white)
        self.update()

    def get_occupancy_map(self):
        map_data = OccupancyMap(np.zeros((self.map_image_.height(), self.map_image_.width()), dtype=np.int8))
        for i in range(self.map_image_.width()):
            for j in range(self.map_image_.height()):
                pixel_value = self.map_image_.pixel(i, j)
                if pixel_value == QColor(Qt.black).rgb():
                    map_data[j, i] = 100
                elif pixel_value == QColor(Qt.gray).rgb():
                    map_data[j, i] = -1
                else:
                    map_data[j, i] = 0
        return map_data

    def start_draw_line(self, pose):
        self.line_start_pose_ = pose

    def end_draw_line(self, pose, is_draw):
        if not self.is_draw_line_:
            self.line_tmp_image_ = self.map_image_
            self.is_draw_line_ = True
        self.map_image_ = self.line_tmp_image_
        painter = QPainter(self.map_image_)
        painter.setPen(QPen(Qt.black, 1, Qt.SolidLine, Qt.RoundCap, Qt.RoundJoin))
        painter.drawLine(self.line_start_pose_, pose)
        if is_draw:
            self.is_draw_line_ = False
        self.update()

    def draw_point(self, point):
        painter = QPainter(self.map_image_)
        painter.setPen(QPen(Qt.black, 1, Qt.SolidLine, Qt.RoundCap, Qt.RoundJoin))
        painter.drawPoint(point)
        self.update()

class MainWindow(QMainWindow):
    def __init__(self):
        super().__init__()
        self.setWindowTitle("Occupancy Map Display")
        self.setGeometry(100, 100, 800, 600)

        self.display = DisplayOccMap("occupancy_map", 0, "parent")
        self.setCentralWidget(self.display)

        # 模拟数据更新
        self.update_display()

    def update_display(self):
        data = np.random.randint(0, 256, (480, 640), dtype=np.uint8)
        occupancy_map = OccupancyMap(data)
        self.display.update_data(occupancy_map)

if __name__ == '__main__':
    app = QApplication(sys.argv)
    window = MainWindow()
    window.show()
    sys.exit(app.exec_())